#include "StdAfx.h"
#include "ImageHandler.h"
#include <algorithm>
#include "HLUtility.h"
using namespace HLCAO;

void  gpuGradientMagAndAngle(float* imgBuffer, int width, int height, float* outMagImage, float* outAngleImage);

ImageHandler::ImageHandler(void):m_CntKeyPointsVec(7, Saliency()), m_depKeyPointsVec(7, Saliency()) 
{
	m_pImage=NULL;
	lp_sobel_ag=NULL;
	lp_sobel_dx=NULL;
	lp_sobel_dy=NULL;
	lp_sobel_mg=NULL;
	m_pByteMat=NULL;
}

ImageHandler::~ImageHandler(void)
{
	releaseAll();
}

bool ImageHandler::loadImage( const char* fileName )
{
	// load pic always as grey iamge
	m_pImage = cvLoadImage(fileName, 0);
	if (!m_pImage)
	{
		MessageBox(NULL, L"failed", L"load image failed", MB_OK);
		return false;
	}
	return true;
}

bool ImageHandler::loadDepth(const char* fileName)
{
	m_pImage = cvLoadImage(fileName, -1);
	if (!m_pImage)
	{
		MessageBox(NULL, L"failed", L"load image failed", MB_OK);
		return false;
	}
	return true;
}


void ImageHandler::releaseImage(IplImage** pImage)
{
	cvReleaseImage( pImage );
	*pImage = NULL;
}

float ImageHandler::getPixelValue( IplImage* pImage, int h, int w )
{
// 	return ((uchar *)(m_pImage->imageData + x*m_pImage->widthStep))[y]; 
	int depth=pImage->depth/8;
	switch (depth)
	{
	case 1:
		return ((uchar *)(pImage->imageData + h*pImage->widthStep))[w]; 
		break;
	case 2:
		return  ((ushort *)(pImage->imageData + h*pImage->widthStep))[w];
		break;
	case 4:
		return ((float *)(pImage->imageData + h*pImage->widthStep))[w]; 
		break;
	default:
		break;
	}
}

void ImageHandler::setGridValue(IplImage* pImage, int jGpos, int iGpos, void* val)
{
	int start_j = GRID_SIZE * jGpos;
	int start_i = GRID_SIZE * iGpos;
	for (int j = start_j; j < start_j+GRID_SIZE; ++j )
	{
		for (int i = start_i; i < start_i+GRID_SIZE; ++i)
		{
			setPixelValue(pImage, j, i, val);
		}
	}
}

void ImageHandler::setPixelValue( IplImage* pImage, int h, int w, void* val )
{
	int depth=pImage->depth/8;
	switch (depth)
	{
	case 1:
		((uchar *)(pImage->imageData + h*pImage->widthStep))[w]=*((uchar *)val);
		break;
	case 2:
		((ushort *)(pImage->imageData + h*pImage->widthStep))[w] = *((ushort *)val);
		break;
	case 4:
		((float *)(pImage->imageData + h*pImage->widthStep))[w]=*((float *)val);
		break;
	default:
		break;
	}
}

IplImage* ImageHandler::getImage()
{
	return m_pImage;
}

void ImageHandler::constructGradientImage()
{
	lp_sobel_dx = cvCreateImage(cvGetSize(m_pImage),IPL_DEPTH_32F,1);
	lp_sobel_dy = cvCreateImage(cvGetSize(m_pImage),IPL_DEPTH_32F,1);
	lp_sobel_mg = cvCreateImage(cvGetSize(m_pImage),IPL_DEPTH_32F,1);
	lp_sobel_ag = cvCreateImage(cvGetSize(m_pImage),IPL_DEPTH_32F,1);

// 	hlTimeCounter timer;
// 	timer.Start();
	cvSobel(m_pImage,lp_sobel_dx,1,0,3);
	cvSobel(m_pImage,lp_sobel_dy,0,1,3);
	cvCartToPolar(lp_sobel_dx,lp_sobel_dy,lp_sobel_mg,lp_sobel_ag,1);
// 	timer.End();
// 	std::cout<<" MagAndAngle calculated: "<<timer.getElapseTime()<<std::endl;

// 	lp_sobel_mg = cvCreateImage(cvGetSize(m_pImage),IPL_DEPTH_32F,1);
// 	lp_sobel_ag = cvCreateImage(cvGetSize(m_pImage),IPL_DEPTH_32F,1);
//  
// 	float* imgBuffer = generate2DBuffer(m_pImage);
// 	int width = m_pImage->width;
// 	int height = m_pImage->height;
// 	float* magBuffer = new float[width*height];
// 	float* angleBuffer = new float[width*height];
// 
// 	hlTimeCounter gpuTimer;
// 	gpuTimer.Start();
// 	gpuGradientMagAndAngle(imgBuffer, width, height, magBuffer, angleBuffer);
// 	gpuTimer.End();
// 	std::cout<<" GPU calculated: "<<gpuTimer.getElapseTime()<<std::endl;
// 
// 	bufferToImage(magBuffer, lp_sobel_mg, width, height);
// 	bufferToImage(angleBuffer, lp_sobel_ag, width, height);
// 
// 	delete[] imgBuffer;
// 	delete[] magBuffer;
// 	delete[] angleBuffer;
}

IplImage* ImageHandler::getSobelDx()
{
	return lp_sobel_dx;
}

IplImage* ImageHandler::getSobelDy()
{
	return lp_sobel_dy;
}

IplImage* ImageHandler::getSobelMg()
{
	return lp_sobel_mg;
}

IplImage* ImageHandler::getSobelAg()
{
	return lp_sobel_ag;
}

void ImageHandler::initGridInfo( )
{
	m_gridWidth = m_pImage->width/GRID_SIZE;
	m_gridHeight = m_pImage->height/GRID_SIZE;
	m_pByteMat = cvCreateMat(m_gridHeight, m_gridWidth, CV_8UC1);
}

uchar ImageHandler::buildDepTemplateBytePattern(GRID_POS hPos, GRID_POS wPos)
{
	Point2D pixPos = getPixPosByGridPos(hPos, wPos);
	int y_start = pixPos.h;
	int x_start = pixPos.w;
	uchar oriByte = 0;
	int	disAngleArry[7] = {0};
	int invalidValCount = 0;

	for (int j = y_start; j< y_start+GRID_SIZE;  ++j)
	{
		for (int i = x_start; i< x_start+GRID_SIZE; ++i)
		{
			ushort depthVal = getPixelValue(m_pImage, j ,i);
			if (depthVal == 0)
				invalidValCount++;
						
			int disAngle = discretizeOrientation(getPixelValue(lp_sobel_ag, j ,i));
			disAngleArry[disAngle] ++;
		}
	}
	//if number of invalid points in depth map is more than a threshold, say 10
	//we set this patch as unreliable
	//or
	//if the average gradient is too small
	if (invalidValCount > 5   )
	{
// 		std::cout<<"bad points: "<<invalidValCount<<std::endl;
		oriByte |= 0x80;
		return oriByte;
	}

	for (int i=0; i<6; ++i)
	{
		if (disAngleArry[i] >= FRETHRESHOLD) 
		{
			oriByte |= (((uchar) 1) << i);
		}
	}
	return oriByte;
}

uchar ImageHandler::buildImgTemplateBytePattern( GRID_POS hPos, GRID_POS wPos)
{
	Point2D pixPos = getPixPosByGridPos(hPos, wPos);
	int y_start = pixPos.h;
	int x_start = pixPos.w;
	std::vector<MagWithPos> magVector;
	uchar oriByte=0;

	for (int j = y_start; j< y_start+GRID_SIZE;  ++j)
	{
		for (int i = x_start; i< x_start+GRID_SIZE; ++i)
		{
			float mag = getPixelValue(lp_sobel_mg, j ,i);
			if (mag>MAGTHRESHOLD)
			{
				MagWithPos tempMag;
				tempMag.mag=mag;
				tempMag.hpos=j;
				tempMag.wpos=i;
				magVector.push_back(tempMag);
			}
		}
	}
	//if empty, set to 1000 0000
	if (magVector.empty())
		oriByte |= 0x80;
	else
	{
		sort(magVector.begin(), magVector.end(), MagCompareFunc);
		for (size_t i=0; i<magVector.size(); ++i)
		{
			if (i>6)
				break;
			float oriVal = getPixelValue(lp_sobel_ag, magVector[i].hpos, magVector[i].wpos);
			int angle = discretizeOrientation(oriVal);
			oriByte |=(((uchar)1)<<angle);
		}
	}
	return oriByte;
}

Point2D ImageHandler::getPixPosByGridPos( GRID_POS hPos, GRID_POS wPos )
{
	Point2D pixLeftTop;
	pixLeftTop.h = hPos*GRID_SIZE;
	pixLeftTop.w = wPos*GRID_SIZE;
	return pixLeftTop;
}

int ImageHandler::discretizeOrientation( float angle, float div )
{
	return (int)(angle/div);
}

bool ImageHandler::MagCompareFunc( const MagWithPos& mag1, const MagWithPos& mag2 )
{
	//remember that default is < operation
	return mag1.mag>mag2.mag;
}

bool ImageHandler::SaliencyCompareFunc(const Saliency& sal1, const Saliency& sal2)
{
	return sal1.hisValue>sal2.hisValue;
}

int ImageHandler::getGridWidth()
{
	return m_gridWidth;
}

int ImageHandler::getGridHeight()
{
	return m_gridHeight;
}

void ImageHandler::generateDepTemplateByteMat()
{
	for (int j=0; j<m_gridHeight; ++j)
	{
		for (int i=0; i<m_gridWidth; ++i)
		{
			CV_MAT_ELEM(*m_pByteMat,uchar,j,i)= buildDepTemplateBytePattern(j,i);
		}
	}
	int a =0;
}

void ImageHandler::generateImgTemplateByteMat()
{
	for (int j=0; j<m_gridHeight; ++j)
	{
		for (int i=0; i<m_gridWidth; ++i)
		{
			CV_MAT_ELEM(*m_pByteMat,uchar,j,i)= buildImgTemplateBytePattern(j, i);
		}
	}
}

CvMat* ImageHandler::getByteMat()
{
	return m_pByteMat;
}

uchar ImageHandler::buildDepTargetBytePattern(GRID_POS hPos, GRID_POS wPos)
{
	Point2D pixPos = getPixPosByGridPos(hPos, wPos);
	int y_start = pixPos.h;
	int x_start = pixPos.w;
	uchar oriByte=0;
	int angleArray[7]={0};
	for (int j = y_start; j< y_start+GRID_SIZE;  ++j)
	{
		for (int i = x_start; i< x_start+GRID_SIZE; ++i)
		{
			int _angle = discretizeOrientation(getPixelValue(lp_sobel_ag, j ,i));
			angleArray[_angle]++;
		}
	}
	int maxCount = 0;
	int maxAngle = 0;
	for (int i=0; i<6; ++i)
	{
		if (angleArray[i] > maxCount)
		{
			maxCount = angleArray[i];
			maxAngle = i;
		}
	}
	oriByte |=((uchar)1)<<maxAngle;
	return oriByte;
}

uchar ImageHandler::buildImgTargetBytePattern( GRID_POS hPos, GRID_POS wPos )
{
	Point2D pixPos = getPixPosByGridPos(hPos, wPos);
	int y_start = pixPos.h;
	int x_start = pixPos.w;
	uchar oriByte=0;
	int maxMag=0;
	int tempJ=0;
	int tempI=0;
	for (int j = y_start; j< y_start+GRID_SIZE;  ++j)
	{
		for (int i = x_start; i< x_start+GRID_SIZE; ++i)
		{
			float mag = getPixelValue(lp_sobel_mg, j ,i);
			if (mag>=maxMag)
			{
				maxMag=mag;
				tempJ=j;
				tempI=i;
			}
		}
	}
	float oriVal = getPixelValue(lp_sobel_ag, tempJ, tempI);
	int angle = discretizeOrientation(oriVal);
	oriByte |=((uchar)1)<<angle;
	return oriByte;
}

void ImageHandler::generateDepTargetByteMat()
{
	for (int j=0; j<m_gridHeight; ++j)
	{
		for (int i=0; i<m_gridWidth; ++i)
		{
			CV_MAT_ELEM(*m_pByteMat,uchar,j,i)= buildDepTargetBytePattern(j,i);
		}
	}
}

void ImageHandler::generateImgTargetByteMat()
{
	for (int j=0; j<m_gridHeight; ++j)
	{
		for (int i=0; i<m_gridWidth; ++i)
		{
			CV_MAT_ELEM(*m_pByteMat,uchar,j,i)= buildImgTargetBytePattern(j,i);
		}
	}
}

void ImageHandler::setImageWithPointer( IplImage* pImage )
{
	this->m_pImage=pImage;
}

void ImageHandler::releaseAll()
{
	if ( m_pImage )
	{
		releaseImage(&m_pImage);
		m_pImage=NULL;
	}
	if (lp_sobel_ag)
	{
		releaseImage(&lp_sobel_ag);
		lp_sobel_ag = NULL;
	}
	if (lp_sobel_dx)
	{
		releaseImage(&lp_sobel_dx);
		lp_sobel_dx = NULL;
	}
	if (lp_sobel_dy)
	{
		releaseImage(&lp_sobel_dy);
		lp_sobel_dy = NULL;
	}
	if (lp_sobel_mg)
	{
		releaseImage(&lp_sobel_mg);
		lp_sobel_mg = NULL;
	}
	if (m_pByteMat)
	{
		cvReleaseMat(&m_pByteMat);
		m_pByteMat = NULL;
	}
}

void ImageHandler::createImage( CvSize size, int depth, int channels )
{
	m_pImage = cvCreateImage(size, depth, channels);
}

void ImageHandler::setMaskImgWithPointer( IplImage* pImage )
{
	m_maskImg = pImage;
}

IplImage* ImageHandler::getMaskImg()
{
	return m_maskImg;
}

bool ImageHandler::isGridInObjectArea( GRID_POS jGPos, GRID_POS iGPos )
{
	int j_start = jGPos * GRID_SIZE;
	int i_start = iGPos * GRID_SIZE;
	for (int j = j_start; j < j_start + GRID_SIZE; ++j)
		for (int i = i_start; i < i_start +GRID_SIZE; ++i)
		{
			if (getPixelValue(m_maskImg, j , i) < 255)
			{
				return false;
			}
		}
	return true;
}

std::vector<Saliency>& ImageHandler::getCntKeyPoints()
{
	return m_CntKeyPointsVec;
}

std::vector<Saliency>& ImageHandler::getDepKeyPoints()
{
	return m_depKeyPointsVec;
}

void ImageHandler::generateCntKeyPoints()
{
	IplImage* sobel_dx = cvCreateImage(cvGetSize(m_maskImg),IPL_DEPTH_32F,1);
	IplImage* sobel_dy = cvCreateImage(cvGetSize(m_maskImg),IPL_DEPTH_32F,1);
	IplImage* sobel_mg = cvCreateImage(cvGetSize(m_maskImg),IPL_DEPTH_32F,1);
	IplImage* sobel_ag = cvCreateImage(cvGetSize(m_maskImg),IPL_DEPTH_32F,1);

	cvSobel(m_maskImg, sobel_dx,1,0,3);
	cvSobel(m_maskImg, sobel_dy,0,1,3);
	cvCartToPolar(sobel_dx,sobel_dy,sobel_mg,sobel_ag,1);

	for (int j = 0; j < m_gridHeight; ++j)
	{
		for (int i = 0; i< m_gridWidth; ++i)
		{
			int angle[8] = {0};
			int maxAngle=0, maxHis =0;
			int j_start = j * GRID_SIZE;
			int i_start = i * GRID_SIZE;
			for (int _j = j_start; _j < j_start + GRID_SIZE; ++_j)
			{
				for (int _i = i_start; _i < i_start +GRID_SIZE; ++_i)
				{
					float mag = getPixelValue(sobel_mg, _j, _i);
					if (mag > MAGTHRESHOLD)
					{
						int disDir = discretizeOrientation(getPixelValue(sobel_ag, _j, _i));
						angle[disDir] ++;
					}
				}
			}
			for (int k=0; k<7; ++k)
			{
				if (angle[k] > maxHis)
				{
					maxHis = angle[k];
					maxAngle = k;
				}
			}
			Saliency saliency;
			saliency.jGPos = j;
			saliency.iGPos = i;
			saliency.hisValue = maxHis;
			saliency.disDir = maxAngle;
			if (saliency.hisValue > (m_CntKeyPointsVec[saliency.disDir]).hisValue)
			{
				m_CntKeyPointsVec[saliency.disDir] = saliency;
			}
		}
	}
	cvReleaseImage(&sobel_ag);
	cvReleaseImage(&sobel_mg);
	cvReleaseImage(&sobel_dx);
	cvReleaseImage(&sobel_dy);
}

void ImageHandler::generateDepKeyPoints()
{
	for (int j = 0; j < m_gridHeight; ++j)
	{
		for (int i = 0; i< m_gridWidth; ++i)
		{
			int angle[8] = {0};
			int maxAngle=0, maxHis =0;
			int j_start = j * GRID_SIZE;
			int i_start = i * GRID_SIZE;
			for (int _j = j_start; _j < j_start + GRID_SIZE; ++_j)
			{
				for (int _i = i_start; _i < i_start +GRID_SIZE; ++_i)
				{
					int disDir = discretizeOrientation(getPixelValue(lp_sobel_ag, _j, _i));
					if (disDir==6)
					{
// 						std::cout<<"aaaa"<<std::endl;
					}
					angle[disDir] ++;
				}
			}
			for (int k=0; k<7; ++k)
			{
				if (angle[k] > maxHis)
				{
					maxHis = angle[k];
					maxAngle = k;
				}
			}
			Saliency saliency;
			saliency.jGPos = j;
			saliency.iGPos = i;
			saliency.hisValue = maxHis;
			saliency.disDir = maxAngle;
			if (saliency.hisValue > m_depKeyPointsVec[saliency.disDir].hisValue && isGridInObjectArea(j, i))
			{
				m_depKeyPointsVec[saliency.disDir] = saliency;
			}
		}
	}
}

float* ImageHandler::generate2DBuffer( IplImage* pImage )
{
	int width = pImage->width;
	int height = pImage->height;
	float* pBuffer = new float[width*height];
	for (int j=0; j<height; ++j)
	{
		for (int i=0; i<width; ++i)
		{
			pBuffer[j*height+i] = ImageHandler::getPixelValue(pImage, j, i) ;
		}
	}
	return pBuffer;
}

void ImageHandler::bufferToImage( float* buffer, IplImage* pImage, int width, int height )
{
	for (int j=0; j< height; ++j)
	{
		for(int i=0; i<width; ++i)
		{
			float val = buffer[j*width+i];
			ImageHandler::setPixelValue(pImage, j, i, &val);
		}
	}
}
